#include "mesh.h"

#if defined(__APPLE__) || defined(MACOSX)
#include <GLUT/glut.h>
#else
#include <gl/glut.h>
#endif

#include <iostream>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <float.h>


Mesh::Mesh(){}
using namespace std;

/************************************************************
 * Fonctions de calcul des normales pour chaque sommet
 ************************************************************/
void Mesh::computeVertexNormals()
{
    //First we set the normals to zero
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
		vertices[i].n = Vec (0.0, 0.0, 0.0);
	}

    //Now we consider each triangle
    for (unsigned int i = 0; i < triangles.size (); i++)
	{
        //Let's determine the three points
		Vec p0 = vertices[triangles[i].v[0]].p;
		Vec p1 = vertices[triangles[i].v[1]].p;
		Vec p2 = vertices[triangles[i].v[2]].p;
		
		//We determine the normal n to the face
		Vec edge01 = p1 -  p0;
        Vec edge02 = p2 -  p0;
		Vec edge12 = p2 -  p1;
        Vec n = edge01 ^ edge02;
        n.normalize();
		
		//We determine weights for the points (2*incident angle * area the triangle)
		double doubleArea = (edge01 ^ edge02).length();
		double w[3];
		Vec e01 = edge01; e01.normalize();
		Vec e02 = edge02; e02.normalize();
		Vec e12 = edge12; e12.normalize();
		w[0] = doubleArea * acos(e01*e02);
		w[1] = doubleArea * acos(-e01*e12);
		w[2] = doubleArea * acos(e12*e02);
		
		//We add the weighted value for each vertex
        for (unsigned int j = 0; j < 3; j++)
		{
            vertices[triangles[i].v[j]].n += w[j]*n;
		}
    }

    //And finally we normalize the normals and determine orthonormal
	//coordinate system vectors for the tangent plane
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
		vertices[i].n.normalize ();
		vertices[i].u = Vec(0,0,1);
		vertices[i].u -= (vertices[i].u * vertices[i].n) * vertices[i].n;
		
		while(vertices[i].u.normalize() == 0)
		{
			vertices[i].u.x += 1;
			vertices[i].u -= (vertices[i].u * vertices[i].n) * vertices[i].n;
		}
		
		vertices[i].v = vertices[i].n ^ vertices[i].u;
		vertices[i].v.normalize();
	}
}

/**************************************************************************
 * Fonctions de calcul des matrices de Weingarten pour chaque sommet
 **************************************************************************/
void Mesh::computeWeingarten()
{
	//First we set the weingarten coefficients to zero
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
		vertices[i].weingarten = Mat(2,2);
	}
    
	vector<double> totalWeights(vertices.size(), 0);
	
    //Now we consider each triangle
    for (unsigned int i = 0; i < triangles.size (); i++)
	{
        //Let's determine the three points
		Vec p0 = vertices[triangles[i].v[0]].p;
		Vec p1 = vertices[triangles[i].v[1]].p;
		Vec p2 = vertices[triangles[i].v[2]].p;
		
		//Then the edges
		Vec edge01 = p1 -  p0;
        Vec edge02 = p2 -  p0;
		Vec edge12 = p2 -  p1;
		
		//Let's also set orthonormal coordinate system vectors for the plane containing the triangle
		Vec u(edge01), v(edge02);
		u.normalize();
		v -= (v*u) * u;
		v.normalize();
		
		//Let's compute the face normal while we're at it
		Vec n = u ^ v;
		n.normalize();
		
		
		//We determine the equations satisfied by the Weingarten matrix for the face
		Mat A(6,3), B(6,1);
		
		A(0,0) = edge01 * u;
		A(0,1) = edge01 * v;
		A(1,1) = A(0,0);
		A(1,2) = A(0,1);
		A(2,0) = edge02 * u;
		A(2,1) = edge02 * v;
		A(3,1) = A(2,0);
		A(3,2) = A(2,1);
		A(4,0) = edge12 * u;
		A(4,1) = edge12 * v;
		A(5,1) = A(4,0);
		A(5,2) = A(4,1);
		
		B(0,0) = (vertices[triangles[i].v[1]].n - vertices[triangles[i].v[0]].n) * u;
		B(1,0) = (vertices[triangles[i].v[1]].n - vertices[triangles[i].v[0]].n) * v;
		B(2,0) = (vertices[triangles[i].v[2]].n - vertices[triangles[i].v[0]].n) * u;
		B(3,0) = (vertices[triangles[i].v[2]].n - vertices[triangles[i].v[0]].n) * v;
		B(4,0) = (vertices[triangles[i].v[2]].n - vertices[triangles[i].v[1]].n) * u;
		B(5,0) = (vertices[triangles[i].v[2]].n - vertices[triangles[i].v[1]].n) * v;
		
		//We get x minimizing ||A*x-B||^2
		Mat t_A = A.transpose();
		Mat x = Mat::gauss(t_A * A, t_A * B);
		
		//Then we build the matrix
		Mat faceWeingarten(2,2);
		faceWeingarten(0,0) = x(0,0);
		faceWeingarten(1,0) = x(1,0);
		faceWeingarten(0,1) = x(1,0);
		faceWeingarten(1,1) = x(2,0);
		
		//We determine weights for the points (2*incident angle * area the triangle)
		double doubleArea = (edge01 ^ edge02).length();
		double w[3];
		Vec e01 = edge01; e01.normalize();
		Vec e02 = edge02; e02.normalize();
		Vec e12 = edge12; e12.normalize();
		w[0] = doubleArea * acos(e01*e02);
		w[1] = doubleArea * acos(-e01*e12);
		w[2] = doubleArea * acos(e12*e02);
		
		//For each vertex, we compute the matrix in the good coordinates and add it with a weight
		Mat temp_mat(2,2), temp_u(2,1), temp_v(2,1);
		Vec rotated_u, rotated_v;
		
        for (unsigned int j = 0; j < 3; j++)
		{
			//Rotation of u and v to put them in the same plane
			rotated_u = rotate(vertices[triangles[i].v[j]].u, vertices[triangles[i].v[j]].n, n);
			rotated_v = rotate(vertices[triangles[i].v[j]].v, vertices[triangles[i].v[j]].n, n);
			
			//We express the coordinate vectors for the point in the coordinate system of the face
			temp_u(0,0) = rotated_u * u;
			temp_u(1,0) = rotated_u * v;
			temp_v(0,0) = rotated_v * u;
			temp_v(1,0) = rotated_v * v;
			
			//We use that to express the Weingarten matrix in the coordinate system of the point
			temp_mat(0,0) = (temp_u.transpose() * faceWeingarten * temp_u)(0,0);
			temp_mat(1,0) = (temp_u.transpose() * faceWeingarten * temp_v)(0,0);
			temp_mat(0,1) = temp_mat(1,0);
			temp_mat(1,1) = (temp_v.transpose() * faceWeingarten * temp_v)(0,0);
			
			totalWeights[triangles[i].v[j]] += w[j];
			vertices[triangles[i].v[j]].weingarten += w[j]*temp_mat;
		}
    }
	
	//And finally we normalize the matrices
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
		vertices[i].weingarten /= totalWeights[i];
	}
}

/**************************************************************************
 * Fonctions de calcul des matrices pour les dérivées de courbure
 **************************************************************************/
void Mesh::computeDerivativeCurvature()
{
	//First we set the derivative matrix coefficients to zero
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
		vertices[i].derivative = Mat(4,2);
	}
    
	vector<double> totalWeights(vertices.size(), 0);
	
    //Now we consider each triangle
    for (unsigned int i = 0; i < triangles.size (); i++)
	{
        //Let's determine the three points
		Vec p0 = vertices[triangles[i].v[0]].p;
		Vec p1 = vertices[triangles[i].v[1]].p;
		Vec p2 = vertices[triangles[i].v[2]].p;
		
		//Then the edges
		Vec edge01 = p1 -  p0;
        Vec edge02 = p2 -  p0;
		Vec edge12 = p2 -  p1;
		
		//Let's also set orthonormal coordinate system vectors for the plane containing the triangle
		Vec u(edge01), v(edge02);
		u.normalize();
		v -= (v*u) * u;
		v.normalize();
		
		//Let's compute the face normal while we're at it
		Vec n = u ^ v;
		n.normalize();
		
		
		//Now, let's express the weingarten matrices in the coordinates of the face
		Mat weingarten[3];
		
		Vec rotated_u, rotated_v;
		Mat temp_mat(2,2), temp_u(2,1), temp_v(2,1);
		for (unsigned int j = 0; j < 3; j++)
		{
			weingarten[j] = Mat(2,2);
			temp_mat = vertices[triangles[i].v[j]].weingarten;
			
			//Rotation of u and v to put them in the same plane as the face
			rotated_u = rotate(vertices[triangles[i].v[j]].u, vertices[triangles[i].v[j]].n, n);
			rotated_v = rotate(vertices[triangles[i].v[j]].v, vertices[triangles[i].v[j]].n, n);
			
			//We express the coordinate vectors for the face in the coordinate system of the point
			temp_u(0,0) = rotated_u * u;
			temp_u(1,0) = rotated_v * u;
			temp_v(0,0) = rotated_u * v;
			temp_v(1,0) = rotated_v * v;
			
			//And we compute the weingarten matrix in the coordinates of the face
			weingarten[j](0,0) = (temp_u.transpose() * temp_mat * temp_u)(0,0);
			weingarten[j](1,0) = (temp_u.transpose() * temp_mat * temp_v)(0,0);
			weingarten[j](0,1) = weingarten[j](1,0);
			weingarten[j](1,1) = (temp_v.transpose() * temp_mat * temp_v)(0,0);
		}
		
		
		
		//We determine the equations satisfied by the derivative matrix for the face (4 degrees of freedom)
		Mat A(9,4), B(9,1);
		
		A(0,0) = edge01 * u;
		A(0,1) = edge01 * v;
		A(1,1) = A(0,0);
		A(1,2) = A(0,1);
		A(2,2) = A(0,0);
		A(2,3) = A(0,1);
		A(3,0) = edge02 * u;
		A(3,1) = edge02 * v;
		A(4,1) = A(3,0);
		A(4,2) = A(3,1);
		A(5,2) = A(3,0);
		A(5,3) = A(3,1);
		A(6,0) = edge12 * u;
		A(6,1) = edge12 * v;
		A(7,1) = A(6,0);
		A(7,2) = A(6,1);
		A(8,2) = A(6,0);
		A(8,3) = A(6,1);
		
		B(0,0) = weingarten[1](0,0) - weingarten[0](0,0);
		B(1,0) = weingarten[1](0,1) - weingarten[0](0,1);
		B(2,0) = weingarten[1](1,1) - weingarten[0](1,1);
		B(3,0) = weingarten[2](0,0) - weingarten[0](0,0);
		B(4,0) = weingarten[2](0,1) - weingarten[0](0,1);
		B(5,0) = weingarten[2](1,1) - weingarten[0](1,1);
		B(6,0) = weingarten[2](0,0) - weingarten[1](0,0);
		B(7,0) = weingarten[2](0,1) - weingarten[1](0,1);
		B(8,0) = weingarten[2](1,1) - weingarten[1](1,1);
		
		//We get x minimizing ||A*x-B||^2
		Mat t_A = A.transpose();
		Mat x = Mat::gauss(t_A * A, t_A * B);
		
		//Then we build two temporary matrices
		Mat tempFaceDerivative1(2,2), tempFaceDerivative2(2,2);
		tempFaceDerivative1(0,0) = x(0,0);
		tempFaceDerivative1(1,0) = x(1,0);
		tempFaceDerivative1(0,1) = x(1,0);
		tempFaceDerivative1(1,1) = x(2,0);
														 
		tempFaceDerivative2(0,0) = x(1,0);
		tempFaceDerivative2(1,0) = x(2,0);
		tempFaceDerivative2(0,1) = x(2,0);
		tempFaceDerivative2(1,1) = x(3,0);
														 
		//We determine weights for the points (2*incident angle * area the triangle)
		double doubleArea = (edge01 ^ edge02).length();
		double w[3];
		Vec e01 = edge01; e01.normalize();
		Vec e02 = edge02; e02.normalize();
		Vec e12 = edge12; e12.normalize();
		w[0] = doubleArea * acos(e01*e02);
		w[1] = doubleArea * acos(-e01*e12);
		w[2] = doubleArea * acos(e12*e02);
		
		//For each vertex, we compute the matrix in the good coordinates and add it with a weight
		Mat temp_derivative(4,2);
        for (unsigned int j = 0; j < 3; j++)
		{
			//Rotation of u and v to put them in the same plane as the face
			rotated_u = rotate(vertices[triangles[i].v[j]].u, vertices[triangles[i].v[j]].n, n);
			rotated_v = rotate(vertices[triangles[i].v[j]].v, vertices[triangles[i].v[j]].n, n);
			
			//We express the coordinate vectors for the point in the coordinate system of the face
			temp_u(0,0) = rotated_u * u;
			temp_u(1,0) = rotated_u * v;
			temp_v(0,0) = rotated_v * u;
			temp_v(1,0) = rotated_v * v;
			
			//We use that to express the derivative matrix in the coordinate system of the point
			temp_derivative(0,0) = (temp_u.transpose() * tempFaceDerivative1 * temp_u)(0,0);
			temp_derivative(1,0) = (temp_u.transpose() * tempFaceDerivative1 * temp_v)(0,0);
			temp_derivative(2,0) = temp_derivative(1,0);
			temp_derivative(3,0) = (temp_v.transpose() * tempFaceDerivative1 * temp_v)(0,0);
			
			temp_derivative(0,1) = (temp_u.transpose() * tempFaceDerivative2 * temp_u)(0,0);
			temp_derivative(1,1) = (temp_u.transpose() * tempFaceDerivative2 * temp_v)(0,0);
			temp_derivative(2,1) = temp_derivative(1,0);
			temp_derivative(3,1) = (temp_v.transpose() * tempFaceDerivative2 * temp_v)(0,0);
			
			totalWeights[triangles[i].v[j]] += w[j];
			vertices[triangles[i].v[j]].derivative += w[j]*temp_derivative;
		}
    }
	
	//And finally we normalize the matrices
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
		vertices[i].derivative /= totalWeights[i];
	}
}


/************************************************************
 * Recentre et ajuste la taille des sommets du maillage
 ************************************************************/
void Mesh::centerAndScaleToUnit () {
    Vec c;
	
    for  (unsigned int i = 0; i < vertices.size (); i++)
        c += vertices[i].p;
    c /= vertices.size ();
	
    double maxD = c.distanceTo(vertices[0].p);
    for (unsigned int i = 0; i < vertices.size (); i++)
	{
        double m = c.distanceTo(vertices[i].p);
        if (m > maxD)
		{
            maxD = m;
		}
    }
	
    for  (unsigned int i = 0; i < vertices.size (); i++)
	{
        vertices[i].p = 2*(vertices[i].p - c) / maxD;
	}
}


/************************************************************
 * Fonctions de dessin
 ************************************************************/
void Mesh::drawSmooth(){

    glBegin(GL_TRIANGLES);

    for (int i=0;i<triangles.size();++i)
    {
        for(int v = 0; v < 3 ; v++)
		{
            glNormal3f(vertices[triangles[i].v[v]].n[0], vertices[triangles[i].v[v]].n[1], vertices[triangles[i].v[v]].n[2]);
            glVertex3f(vertices[triangles[i].v[v]].p[0], vertices[triangles[i].v[v]].p[1] , vertices[triangles[i].v[v]].p[2]);
        }

    }
    glEnd();
}

void Mesh::drawWithColors(const std::vector<Vec> & colors){

    glBegin(GL_TRIANGLES);

    for (int i=0;i<triangles.size();++i)
    {
        for(int v = 0; v < 3 ; v++)
		{
			const Vec &color = colors[triangles[i].v[v]];
            glColor3f(color[0], color[1], color[2]);
			glNormal3f(vertices[triangles[i].v[v]].n.x, vertices[triangles[i].v[v]].n.y, vertices[triangles[i].v[v]].n.z);
            glVertex3f(vertices[triangles[i].v[v]].p.x, vertices[triangles[i].v[v]].p.y , vertices[triangles[i].v[v]].p.z);
        }
    }
    glEnd();
}

bool testIntersect(const Vec & src, const Vec & dir,  
					  const Vec & p0, const Vec & p1, const Vec & p2, 
					  double * beta, double * gamma, double * t)
{

	const double EPSILON=0.00001;
	double A= Vec::det(p0-p1,p0-p2,dir);

	*beta= Vec::det(p0-src,p0-p2,dir)/A;

	if (*beta<-EPSILON)
		return false;
	else if (*beta<0)
		*beta=0;
	else if (*beta>1+EPSILON)
		return false;
	else if (*beta>1)
		*beta=1;

	*gamma= Vec::det(p0-p1, p0-src, dir)/A;

	if (*gamma<-EPSILON)
		return false;
	else if (*gamma<0)
		*gamma=0;
	else if (*gamma>1+EPSILON)
		return false;
	else if (*gamma>1)
		*gamma=1;

	if (*gamma+*beta>1+EPSILON)
		return false;
	else if (*gamma+*beta>1.0)
	{
		//hack something reasonable
		double temp(double(1.0)-*gamma-*beta);
		temp/=2.0;
		*beta+=temp;
		*gamma+=temp;
	}
	*t = Vec::det(p0-p1, p0-p2, p0-src)/A;
	
	if (*t<0)
		return false;
	
	return true;
}



int Mesh::getClosestVertexIndex(const Vec &origin, const Vec &direction)
{
	double minT=FLT_MAX;
	double minBeta=FLT_MAX;
	double minGamma=FLT_MAX;
	unsigned int minFh=0;

	double tT=FLT_MAX;
	double betaT=FLT_MAX;
	double gammaT=FLT_MAX;
	bool hit=false;

	for (unsigned int i=0; i<triangles.size();++i)
	{
		if (testIntersect(origin, direction, vertices[triangles[i].v[0]].p, vertices[triangles[i].v[1]].p, vertices[triangles[i].v[2]].p, &betaT, &gammaT, &tT))
		{
			hit=true;
			if (tT<minT)
			{
				minT=tT;
				minBeta=betaT;
				minGamma=gammaT;
				minFh=i;
			}
		}
	}

	double beta, gamma, t;
	int fh=minFh;
	
	beta=minBeta;
	gamma=minGamma;
	t=minT;
	fh=minFh;
	
	if (hit)
	{
		if (beta<0.5)
		{
			//p0 or p2
			if(gamma<0.5)
				return triangles[minFh].v[0];
			else
				return triangles[minFh].v[2];
		}
		else
		{
			if(gamma<0.5)
				return triangles[minFh].v[1];
			else
				return triangles[minFh].v[2];
		}
	}
	else
		return -1;
	
}	

void Mesh::draw(){
    glBegin(GL_TRIANGLES);

    for (int i=0;i<triangles.size();++i)
    {
        Vec edge01 = vertices[triangles[i].v[1]].p -  vertices[triangles[i].v[0]].p;
        Vec edge02 = vertices[triangles[i].v[2]].p -  vertices[triangles[i].v[0]].p;
        Vec n = edge01 ^ edge02;
        n.normalize ();
        glNormal3f(n[0], n[1], n[2]);
        for(int v = 0; v < 3 ; v++){
            glVertex3f(vertices[triangles[i].v[v]].p.x, vertices[triangles[i].v[v]].p.y , vertices[triangles[i].v[v]].p.z);
        }
    }
    glEnd();
}


/************************************************************
 * Fonctions de chargement du maillage
 ************************************************************/
bool Mesh::loadMesh(const char * filename)
{
	std::vector<int> vhandles;

    const unsigned int LINE_LEN=256;
    char s[LINE_LEN];
    FILE * in;
#ifdef WIN32
    errno_t error=fopen_s(&in, filename,"r");
    if (error!=0)
#else
        in = fopen(filename,"r");
    if (!(in))
#endif
        return false;

    float x, y, z;

    while(in && !feof(in) && fgets(s, LINE_LEN, in))
    {
        // material file
        // vertex
        if (strncmp(s, "v ", 2) == 0)
        {
            if (sscanf(s, "v %f %f %f", &x, &y, &z))
                vertices.push_back(Vertex(Vec(x,y,z)));
        }
        // face
        else if (strncmp(s, "f ", 2) == 0)
        {
            int component(0), nV(0);
            bool endOfVertex(false);
            char *p0, *p1(s+2); //place behind the "f "

            vhandles.clear();

            while (*p1 == ' ') ++p1; // skip white-spaces

            while (p1)
            {
                p0 = p1;

                // overwrite next separator

                // skip '/', '\n', ' ', '\0', '\r' <-- don't forget Windows
                while (*p1 != '/' && *p1 != '\r' && *p1 != '\n' &&
                       *p1 != ' ' && *p1 != '\0')
                    ++p1;

                // detect end of vertex
                if (*p1 != '/') endOfVertex = true;

                // replace separator by '\0'
                if (*p1 != '\0')
                {
                    *p1 = '\0';
                    p1++; // point to next token
                }

                // detect end of line and break
                if (*p1 == '\0' || *p1 == '\n')
                    p1 = 0;


                // read next vertex component
                if (*p0 != '\0')
                {
                    switch (component)
                    {
                    case 0: // vertex
                        vhandles.push_back(atoi(p0)-1);
                        break;

                    case 1: // texture coord
                        //assert(!vhandles.empty());
                        //assert((unsigned int)(atoi(p0)-1) < texcoords.size());
                        //_bi.set_texcoord(vhandles.back(), texcoords[atoi(p0)-1]);
                        break;

                    case 2: // normal
                        //assert(!vhandles.empty());
                        //assert((unsigned int)(atoi(p0)-1) < normals.size());
                        //_bi.set_normal(vhandles.back(), normals[atoi(p0)-1]);
                        break;
                    }
                }

                ++component;

                if (endOfVertex)
                {
                    component = 0;
                    nV++;
                    endOfVertex = false;
                }
            }


            if (vhandles.size()>3)
            {
                //model is not triangulated, so let us do this on the fly...
                //to have a more uniform mesh, we add randomization
                unsigned int k=(false)?(rand()%vhandles.size()):0;
                for (unsigned int i=0;i<vhandles.size()-2;++i)
                {
                    triangles.push_back(Triangle(vhandles[(k+0)%vhandles.size()],vhandles[(k+i+1)%vhandles.size()],vhandles[(k+i+2)%vhandles.size()]));
                }
            }
            else if (vhandles.size()==3)
            {
                triangles.push_back(Triangle(vhandles[0],vhandles[1],vhandles[2]));
            }
            else
            {
                printf("TriMesh::LOAD: Unexpected number of face vertices (<3). Ignoring face");
            }
        }
        memset(&s, 0, LINE_LEN);
    }
    fclose(in);

    centerAndScaleToUnit ();
    computeVertexNormals();
	computeWeingarten();
	//computeDerivativeCurvature();
    return true;
}
